// --------------------------------------------------------------------------------------------------------------------
// <copyright file="InverseKinematicsFilter.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Kinematics
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.Numerics;
    using ShoNS.Array;

    /// <summary>
    /// An abstract class for an IK filter.
    /// This base class sets up the infrastructural routines for ranking
    /// and leaves the responsibility of score calculation to the derived
    /// classes.
    /// Different score calculations indicate different ranking preferences
    /// and can end up with different ranking results.
    /// </summary>
    public abstract class InverseKinematicsFilter
    {
        /// <summary>
        /// A threshold used to decide the scores of two joint configurations
        /// are actually the same.
        /// </summary>
        public const double DefaultScoreSimilarityThreshold = 1e-3;

        /// <summary>
        /// A threshold used to decide whether two joint configurations are actually at the same angle.
        /// </summary>
        public const double DefaultJointAngleDifferenceThreshold = 0.5 * MathConstants.Degrees2Radians;

        /// <summary>
        /// Calculate the sum of difference in each joint between two configurations
        /// </summary>
        /// <param name="configuration1">The first configuration</param>
        /// <param name="configuration2">The second configuration</param>
        /// <returns>The sum of the absolute difference in each joint</returns>
        public static double JointConfigurationDifference(DoubleArray configuration1, DoubleArray configuration2)
        {
            double diff = 0;
            for (int i = 0; i < configuration1.size0; ++i)
            {
                diff += Math.Abs(configuration1[i, 0] - configuration2[i, 0]);
            }

            return diff;
        }

        /// <summary>
        /// Calculate the score of a joint configuration
        /// </summary>
        /// <param name="jointConfiguration">A joint configuration</param>
        /// <returns>A value indicating the score of the configuration, based on which
        /// configurations are sorted</returns>
        public abstract double ScoreCalculator(DoubleArray jointConfiguration);

        /// <summary>
        /// Clean up duplicated joint configurations.
        /// We decide two joint configurations are the same when their scores are very close to each other.
        /// In this method, it is assumed that the solutions have been ranked in order.
        /// We only look at two neighboring joint configurations in the list.
        /// </summary>
        /// <param name="jointConfiguration">A list of joint configurations</param>
        /// <param name="scores">Scores of the joint configurations of the output</param>
        /// <param name="configurationScoreThreshold">A threshold used to decide whether two configurations are
        /// the same depending on their scores</param>
        /// <param name="jointDifferenceThreshold">A threshold used to decide whether two joint configurations are
        /// the same considering the difference of all the joint angles.</param>
        /// <returns>A list of joint configurations without duplicated ones</returns>
        public virtual List<DoubleArray> SortJointConfigurationAndCleanUpDuplicated(
            List<DoubleArray> jointConfiguration,
            out double[] scores,
            double configurationScoreThreshold = InverseKinematicsFilter.DefaultScoreSimilarityThreshold,
            double jointDifferenceThreshold = InverseKinematicsFilter.DefaultJointAngleDifferenceThreshold)
        {
            // if there is no need to sort or clean
            if (jointConfiguration.Count == 0)
            {
                scores = new double[0];
                return jointConfiguration;
            }

            // create a list for storing the scores
            double[] tempScores;

            // sort all the joint configurations and get the scores as well
            List<DoubleArray> sortedJointConfigurations = this.SortJointConfiguration(jointConfiguration, out tempScores);

            // At this point, all the joint configurations are sorted.
            // Their scores are in ascending order.
            // The following we will assemble a new list without duplicated joint configurations.
            List<DoubleArray> cleanedUpJointConfigurations = new List<DoubleArray>();

            // record the first one, which is always a valid solution
            DoubleArray justAddedConfig = sortedJointConfigurations[0];
            cleanedUpJointConfigurations.Add(justAddedConfig);
            double justAddedScore = tempScores[0];

            // Iterate through the solutions, find those that are not the same, and insert them into the new list.
            // To compare two configurations, we first look at their scores:
            // if their scores differ greatly, they cannot be the same;
            // if their scores are close, they may be the same, so we run an extensive test between the two joint
            // configurations to decide they are actually the same;
            // if the extensive test reports that they are actually the same, we ignore the current potential
            // joint configuration and keep the one that is already in the list because the one in the list has
            // a better score than the potential one.
            for (int i = 1; i < sortedJointConfigurations.Count; ++i)
            {
                // check their general scores
                if (Math.Abs(tempScores[i] - justAddedScore) < configurationScoreThreshold)
                {
                    if (JointConfigurationDifference(justAddedConfig, sortedJointConfigurations[i]) < jointDifferenceThreshold)
                    {
                        // tagging this configuration as removed
                        tempScores[i] = double.NaN;

                        // and ignore this solution since it is already in the list
                        continue;
                    }
                }

                // these two are not the same joint configurations
                justAddedConfig = sortedJointConfigurations[i];
                cleanedUpJointConfigurations.Add(justAddedConfig);
                justAddedScore = tempScores[i];
            }

            // create the score list for output
            scores = new double[cleanedUpJointConfigurations.Count];
            int j = 0;

            // fill in the score list
            for (int i = 0; i < tempScores.Length; ++i)
            {
                if (!double.IsNaN(tempScores[i]))
                {
                    scores[j++] = tempScores[i];
                }
            }

            return cleanedUpJointConfigurations;
        }

        /// <summary>
        /// Sort the joint configurations
        /// </summary>
        /// <param name="jointConfiguration">Original list of joint configurations </param>
        /// <param name="scores">The output scores for the joint configurations.</param>
        /// <returns>A list of sorted joint configurations</returns>
        public List<DoubleArray> SortJointConfiguration(List<DoubleArray> jointConfiguration, out double[] scores)
        {
            // calculate scores for output
            scores = new double[jointConfiguration.Count];

            // sort all the joint configurations
            // create a ranking list to store the scores along with the index to the original data
            List<Tuple<double, int>> rankingList = new List<Tuple<double, int>>(jointConfiguration.Count);
            for (int i = 0; i < jointConfiguration.Count; ++i)
            {
                // cache scores and assemble an auxiliary tuple list for sorting
                rankingList.Add(new Tuple<double, int>(this.ScoreCalculator(jointConfiguration[i]), i));
            }

            // sort them based on the cached scores
            rankingList.Sort(this.CompareTwoTuples);

            // assemble the result
            List<DoubleArray> sortedJointConfiguration = new List<DoubleArray>(jointConfiguration.Count);
            for (int i = 0; i < jointConfiguration.Count; ++i)
            {
                // the sorted joint configurations
                sortedJointConfiguration.Add(jointConfiguration[rankingList[i].Item2]);

                // re-arrange scores
                scores[i] = rankingList[i].Item1;
            }

            return sortedJointConfiguration;
        }

        /// <summary>
        /// Compare two tuples
        /// </summary>
        /// <param name="t1">First tuple</param>
        /// <param name="t2">Second tuple</param>
        /// <returns>A value indicating which tuple has a larger score</returns>
        private int CompareTwoTuples(Tuple<double, int> t1, Tuple<double, int> t2)
        {
            double score1 = t1.Item1;
            double score2 = t2.Item1;

            if (score1 > score2)
            {
                return 1;
            }
            else if (score1 == score2)
            {
                return 0;
            }
            else
            {
                return -1;
            }
        }
    }
}
